#define _CRT_SECURE_NO_WARNINGS 1
#include<iostream>
#include<opencv2/opencv.hpp>
#include<opencv2/opencv_modules.hpp>
#include<opencv2/highgui.hpp>
#include<math.h>
#include<vector>
#include<string>
#include<cstring>
#include"AttitudeSolution.h"
using namespace cv;
using namespace std;


/***********************************************************************************************************************
*函数名：位姿解算函数：Similarity
*函数功能描述：算出装甲板相对小车的偏航角和俯仰角和距离
* 函数参数：RotatedRect类，为装甲板上的左灯条和右灯条
* 函数返回值：储存了偏航角、俯仰角和距离的double数组
* 作者：温浩然
* 修改日期：暂无
* 修改原因：暂无
************************************************************************************************************************/
double* AttitudeSolution::Similarity(Rect rect)
{
    Point center;
    center.x = rect.x + cvRound(rect.height / 2);//矩形中心的x坐标
    center.y = rect.y + cvRound(rect.width / 2);//矩形中心的y坐标
    double width_minus = (center.x - 360) * 3.75;            //根据相机参数，利用相似原理，算出装甲板灯条中心点实际相对小车的偏移量
    double tan_cita = width_minus / 8000;                                 //利用数学原理，根据相机参数算出偏航角的tan值
    double arctan_cita = atan(tan_cita) * 57.3;                  //算出偏航角
    double angle[3];
    angle[0] = arctan_cita;
    double length_minus = (center.y - 240) * 3;
    double tan_alpha = length_minus / 8000;
    double arctan_alpha = atan(tan_alpha) * 57.3;                 //算俯仰角，原理同偏航角一样
    angle[1] = arctan_alpha;
    double s_camerax, s_reality, d, s_cameray, s_camera, length;
    d = sqrt(length_minus * length_minus + width_minus * width_minus + 8000 * 8000);//根据相机参数设置的数字，具体原理在说明文档里
    s_camerax = rect.height * 3;
    s_cameray = rect.width * 3.75;
    s_camera = sqrt(s_camerax * s_camerax + s_cameray * s_cameray);
    s_reality = 67500;
    length = s_reality * d / s_camera;//求出距离
    angle[2] = length;
    this->distance_similarity = length;
    this->pitch_angle_similarity = arctan_alpha;
    this->yaw_angle_similarity = arctan_cita;
    return angle;
}

/***********************************************************************************************************************
*函数名：位姿解算函数：Solvepnp
*函数功能描述：算出装甲板相对小车的偏航角和俯仰角和距离
* 函数参数：Rect类，为匹配好的矩形
* 函数返回值：储存了偏航角、俯仰角和距离的double数组
* 作者：温浩然
* 修改日期：暂无
* 修改原因：暂无
************************************************************************************************************************/
double* AttitudeSolution::Solvepnp(Rect rect)
{
    RotatedRect RRect = RotatedRect(Point(rect.width + 0.5 * rect.y, rect.height + 0.5 * rect.x), Size(rect.width, rect.height), 0);//将Rect类构造为RotatedRect类
    Mat rot_vector, translation_vector;
    vector<Point2f> object2d_point;
    cv::Point2f vertices[4];
    RRect.points(vertices);//把矩形的四个点复制给四维点向量
    cv::Point2f lu, ld, ru, rd;
    std::sort(vertices, vertices + 4, [](const cv::Point2f& p1, const cv::Point2f& p2) { return p1.x < p2.x; });//从4个点的第一个到最后一个进行排序
    if (vertices[0].y < vertices[1].y) {//判断左边和右边的上下角点
        lu = vertices[0];
        ld = vertices[1];
    }
    else {
        lu = vertices[1];
        ld = vertices[0];
    }
    if (vertices[2].y < vertices[3].y) {
        ru = vertices[2];
        rd = vertices[3];
    }
    else {
        ru = vertices[3];
        rd = vertices[2];
    }
    object2d_point.clear();
    object2d_point.push_back(lu);
    object2d_point.push_back(ru);
    object2d_point.push_back(rd);
    object2d_point.push_back(ld);
    std::vector<cv::Point3f> point3d;
    float half_x = 11.0f / 2.0f;
    float half_y = 5.77f / 2.0f;
    point3d.push_back(Point3f(-half_x, half_y, 0));
    point3d.push_back(Point3f(half_x, half_y, 0));
    point3d.push_back(Point3f(half_x, -half_y, 0));
    point3d.push_back(Point3f(-half_x, -half_y, 0));
    cv::Mat rot;
    cv::Mat trans;
    Mat cam_matrix = (Mat_<double>(3, 3) << 1285.3517927598091, 0, 319.44768628958542, 0, 1279.2339468697937, 239.29354061292258, 0, 0, 1);//相机参数
    Mat distortion_coeff = (Mat_<double>(5, 1) << -0.63687295852461456, -1.9748008790347320, 0.030970703651800782, 0.0021944646842516919, 0);
    cv::solvePnP(point3d, object2d_point, cam_matrix, distortion_coeff, rot, trans);
    double tx = trans.at<double>(0, 0);
    double ty = trans.at<double>(1, 0);
    double tz = trans.at<double>(2, 0);
    double dis = sqrt(tx * tx + ty * ty + tz * tz);
    double yaw_angle = atan((double)tx / ty);
    double pitch_angle = atan((double) tz / ty);
    this->distance_pnp = dis;
    this->pitch_angle_pnp = pitch_angle;
    this->yaw_angle_pnp = yaw_angle;
    double xyz[3];
    xyz[0] = yaw_angle;
    xyz[1] = pitch_angle;
    xyz[2] = dis;
    return xyz;
}